#include <fstream>
#include <unistd.h>
#include <opencv2/opencv.hpp>
#include "ubt_mqtt/map_manager.h"
#include "ubt_mqtt/base64.h"
#include "ament_index_cpp/get_package_share_directory.hpp"

namespace ubt {

CMapManager::CMapManager()
  :_gmapping_launch_status(false)
{
    _origin_map_pose.x = 0.0;
    _origin_map_pose.y = 0.0;
    _origin_map_pose.theta = 0.0;

    _map_height = 0.0;
    _map_width = 0.0;
    _map_resolution = 0.025;

    ubt_yaml::getValue(ConfigDataInfo::Instance().getMapYamlFilePath(), "", _map_origin_info);
    RCLCPP_INFO_STREAM(node->get_logger(),"map image: " << _map_origin_info.image);

    // if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_AMR)
    // {
    //     std::string path = ament_index_cpp::get_package_share_directory("gmapping");
    //     _gmapping_ros = path + "/launch/demo_gmapping.launch";
    // }
    // else
    // {
    //     _gmapping_ros = "";
    // }
}

CMapManager::~CMapManager()
{
}

bool CMapManager::StartGmappingLaunch()
{
    // if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_AMR)
    // {
    //     if (!_gmapping_launch_status)
    //     {
    //         _gmapping_launch_status = true;
    //         std::thread launch_thread(&CMapManager::ThreadProc, this);
    //         launch_thread.detach();
    //     }
    //     else
    //     {
    //         RCLCPP_ERROR_STREAM(node->get_logger(),"gmapping launch already start !!!");
    //     }
    // }

    return true;
}

void CMapManager::ThreadProc()
{
    // std::string gmapping_launch = "roslaunch " + _gmapping_ros + " &";
    // system(gmapping_launch.c_str());
    // //execlp("roslaunch", "roslaunch", _gmapping_ros.c_str(), nullptr);
    // _gmapping_launch_status = false;
}

void CMapManager::UpdateMapInfo()
{
    std::vector<int> origin;
    cv::Mat mat = cv::imread(ConfigDataInfo::Instance().getMapImageFilePath());

    ubt_yaml::getValue(ConfigDataInfo::Instance().getMapYamlFilePath(), "", _map_origin_info);

    _map_width = mat.cols;
    _map_height = mat.rows;

    _origin_map_pose.x = _map_origin_info.origin.x;
    _origin_map_pose.y = _map_origin_info.origin.y;

    std::vector<std::string> spilits = ubt_utils::split(_map_origin_info.id, "_");
    _map_id =  (spilits.size() >= 2) ? spilits[1] : " ";

    RCLCPP_INFO_STREAM(node->get_logger(),"map width: " << _map_width << ", height: " << _map_height);
    RCLCPP_INFO_STREAM(node->get_logger(),"map pose x: " << _origin_map_pose.x << ", y: " << _origin_map_pose.y);
    RCLCPP_INFO_STREAM(node->get_logger(),"map id: " << _map_id);
}

bool CMapManager::UpdateLocalPointInfo(const Json::Value& mapPoints)
{
    bool hasPoints = false;
    YAML::Node node;
    std::string localpointinfo;

    for (int i = 0; i < mapPoints.size(); i++)
    {
        Json::Value point = mapPoints[i];
        std::string type = point["type"].asString();
        if (type == "charge-point" || type == "parking-point")
        {
            location_pose_t pose;
            location_pose_t destPose;

            pose.x = point["x"].asFloat();
            pose.y = point["y"].asFloat();
            pose.theta = point["t"].asFloat();

            MapToWorld(pose, destPose);
            localpointinfo = "name=" + point["id"].asString() + "&type=" + type + "&x=" + std::to_string(destPose.x) + \
                            "&y=" + std::to_string(destPose.y) + "&theta=" + std::to_string(destPose.theta) + ";";

        }
    }

    node["system"]["init_poses"] = localpointinfo;

    std::ofstream fout(ConfigDataInfo::Instance().getLocalPointFilePath().c_str());
    fout << node;
    fout.close();

    return true;
}

/**
 *
"image": /home/ubt/ubt_agv_install/cfg/maps/test2000_map.pgm
"resolution": 0.025000
"origin": [-4.400000, -5.200000, 0.000000]
"negate": 0
"occupied_thresh": 0.65
"free_thresh": 0.196
"map": ["V_82_1.0.51", "\u8c03\u5ea6\u56de\u5145", "DT202000082"]
"map_hash": xx
 */
bool CMapManager::UpdateMapOriginFile()
{
    YAML::Node map_node;

    if (ConfigDataInfo::Instance().getRobotAlgorithmType() != ROBOT_ALGORITHN_TYPE_QRCODE)
    {
        map_node["image"] = _map_origin_info.image;
        map_node["occupied_thresh"] = _map_origin_info.occupied_thresh;
        map_node["free_thresh"] = _map_origin_info.free_thresh;
        map_node["negate"] = _map_origin_info.negate;

        map_node["origin"].push_back(_map_origin_info.origin.x);
        map_node["origin"].push_back(_map_origin_info.origin.y);
        map_node["origin"].push_back(_map_origin_info.origin.theta);
    }

    map_node["map"].push_back(_map_origin_info.id);
    map_node["map"].push_back(_map_origin_info.name);
    map_node["map"].push_back(_map_origin_info.code);

    map_node["map_hash"] = _map_origin_info.map_hash;

    std::ofstream fout(ConfigDataInfo::Instance().getMapYamlFilePath().c_str());
    fout << map_node;
    fout.close();

    return true;
}

bool CMapManager::SaveMapImage(const std::string& mapdata)
{
    try
    {
        std::vector<int> params;
        std::string base_map = mapdata;
        std::string imagedata = decodeBase64(ubt_utils::ReplaceStringAll(base_map, "data:image/png;base64,", ""));
        std::vector<char> base64_img(imagedata.begin(), imagedata.end());

        //cv::Mat img = cv::imdecode(base64_img, CV_LOAD_IMAGE_GRAYSCALE);
        cv::Mat img = cv::imdecode(base64_img, cv::IMREAD_GRAYSCALE);

        params.push_back(cv::IMWRITE_JPEG_QUALITY);
        params.push_back(0);
        cv::imwrite(ConfigDataInfo::Instance().getMapImageFilePath(), img, params);
    }
    catch(const std::exception& e)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"save map image error: " << e.what() << '\n');
        return false;
    }

    return true;
}

bool CMapManager::HandleMapInfoData(const Json::Value &map_data, const std::function<void(void)> notifycallback)
{
    try
    {
        if (_map_origin_info.map_hash != map_data["map_hash"].asString() && map_data["map_type"].asString() != "QRCODE")
        {
            if (!SaveMapImage(map_data["map"].asString()))
            {
                return false;
            }

            if (notifycallback)
            {
                notifycallback();
            }
        }

        // 二维码地图没有地图初始位置信息
        if (map_data["map_type"].asString() != "QRCODE")
        {
            Json::Value yaml_json;

            if (map_data["yaml"].type() == Json::ValueType::stringValue)
            {
                ubt_utils::PraseJsonString(map_data["yaml"].asString(), yaml_json);
            }
            else
            {
                yaml_json = map_data["yaml"];
            }
            {
                _map_origin_info.image = ConfigDataInfo::Instance().getMapImageFilePath();
                _map_origin_info.negate = std::atof(yaml_json["negate"].asString().c_str());
                _map_origin_info.occupied_thresh = std::atof(yaml_json["occupied_thresh"].asString().c_str());
                _map_origin_info.free_thresh = std::atof(yaml_json["free_thresh"].asString().c_str());
                _map_origin_info.origin.x = yaml_json["origin"][0].asDouble();
                _map_origin_info.origin.y = yaml_json["origin"][1].asDouble();
                _map_origin_info.origin.theta = yaml_json["origin"][2].asDouble();
            }
        }
        _map_origin_info.id = map_data["map_id"].asString();
        _map_origin_info.name = map_data["map_name"].asString();
        _map_origin_info.code = map_data["map_code"].asString();
        _map_origin_info.map_hash = map_data["map_hash"].asString();

        // update map yaml file
        UpdateMapOriginFile();
        UpdateMapInfo();
        UpdateLocalPointInfo(map_data["map_points"]);
    }
    catch(const std::exception& e)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"prase map info error: " << e.what() << '\n');
        return false;
    }

    return true;
}

bool CMapManager::WorldToMap(const location_pose_t &srcPose, location_pose_t &destPose)
{
    if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_QRCODE)
    {
        destPose.x = srcPose.x * 1000.0;
        destPose.y = srcPose.y * 1000.0;
    }
    else
    {
        destPose.x = (srcPose.x - _origin_map_pose.x) / _map_resolution;
        destPose.y = _map_height - (srcPose.y - _origin_map_pose.y) / _map_resolution;
    }

    destPose.theta = srcPose.theta;
    return true;
}

bool CMapManager::MapToWorld(const location_pose_t &srcPose, location_pose_t &destPose)
{
    if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_QRCODE)
    {
        destPose.x = srcPose.x / 1000.0;
        destPose.y = srcPose.y / 1000.0;
    }
    else
    {
        destPose.x = srcPose.x * _map_resolution + _origin_map_pose.x;
        destPose.y = _origin_map_pose.y + (_map_height - srcPose.y) * _map_resolution;
    }

    destPose.theta = srcPose.theta;
    return true;
}

}


